/**
******************************************************************************
* @file    qmsg.h
* @author  Lucien
* @version V1.0.0
* @date    10/31/2018
* @brief
******************************************************************************
* @attention
*
*
*
******************************************************************************
*/
#include <string.h>
#include "qmsg.h"
#include "stm32l4xx_hal.h"

#define disable_irqs() __disable_irq();
#define enable_irqs() __enable_irq();


/* msg module init */
void msg_init(queue_msg_t *msg_head, msg_t* msgGrp, uint16_t grp_size)
{
    OS_Q *pq;

    if((msg_head == NULL) || (msgGrp == NULL))
        return;

    pq = (OS_Q*)(&(msg_head->q_msg));

    disable_irqs();
    msg_head->msgGrp = msgGrp;
    pq->osq_start = msgGrp;
    pq->osq_end = msgGrp + grp_size;
    pq->osq_in = msgGrp;
    pq->osq_out = msgGrp;
    pq->osq_entries = 0u;
    pq->osq_size = grp_size;
    enable_irqs();
}

/**
* @brief   Post a msg to a msg queue tail, which can be executed in a right place
* @param   [in] handler: a pointer to function
*          [in] param: function parameters
* @return  OS_ERR_Q_FULL           The msg queue is full.
*          OS_ERR_INVALID_PARAM    Handler is NULL
*          OS_ERR_NONE             None error
* */
uint16_t msg_post(queue_msg_t *msg_head, msg_handler_t handler, void *arg)
{
    OS_Q *pq;
    //pq = &q_msg;
    pq = (OS_Q*)(&(msg_head->q_msg));

    disable_irqs();

    /* Make sure queue is not full                  */
    if(pq->osq_entries >= pq->osq_size)
    {
        enable_irqs();
        return (OS_ERR_Q_FULL);
    }

    pq->osq_in->handler = handler;
    pq->osq_in->param = arg;
    pq->osq_in++;
    /* Update the nbr of entries in the queue       */
    pq->osq_entries++;

    /* Wrap IN ptr if we are at end of queue        */
    if(pq->osq_in == pq->osq_end)
    {
        pq->osq_in = pq->osq_start;
    }

    enable_irqs();
    return (OS_ERR_NONE);

}

/**
* @brief   Post a msg to a msg queue front, which can be executed in a right place
* @param   [in] handler: a pointer to function
*          [in] param: function parameters
* @return  OS_ERR_Q_FULL           The msg queue is full.
*          OS_ERR_INVALID_PARAM    Handler is NULL
*          OS_ERR_NONE             None error
* */
uint16_t msg_post_front(queue_msg_t *msg_head, msg_handler_t handler, void *arg)
{
    OS_Q *pq;

    pq = (OS_Q*)(&(msg_head->q_msg));

    if(handler == NULL)
        return (OS_ERR_INVALID_PARAM);

    disable_irqs();

    /* Make sure queue is not full                   */
    if(pq->osq_entries >= pq->osq_size)
    {
        enable_irqs();
        return (OS_ERR_Q_FULL);
    }

    /* Wrap OUT ptr if we are at the 1st queue entry */
    if(pq->osq_out == pq->osq_start)
    {
        pq->osq_out = pq->osq_end;
    }

    pq->osq_out--;
    /* Insert message into queue                     */
    pq->osq_out->handler = handler;
    pq->osq_in->param = arg;

    pq->osq_entries++;
    enable_irqs();
    return (OS_ERR_NONE);

}

/**
* @brief   accept a msg form queue
* @param   [out] pmsg: a pointer to receive a massage
* @return  OS_ERR_Q_EMPTY      The msg queue is empty.
*          OS_ERR_NONE             None error
* */
uint16_t msg_post_accept(queue_msg_t *msg_head, msg_t *pmsg)
{
    OS_Q *pq;

    pq = (OS_Q*)(&(msg_head->q_msg));

    disable_irqs();

    /* See if any messages in the queue                   */
    if(pq->osq_entries > 0u)
    {
        *pmsg = *(pq->osq_out);
        /* Yes, extract oldest message from the queue         */
        pq->osq_out++;
        /* Update the number of entries in the queue          */
        pq->osq_entries--;

        /* Wrap OUT pointer if we are at the end of the queue */
        if(pq->osq_out == pq->osq_end)
        {
            pq->osq_out = pq->osq_start;
        }

        enable_irqs();
        return OS_ERR_NONE;
    }
    else
    {
        enable_irqs();
        /* Queue is empty */
        return OS_ERR_Q_EMPTY;
    }
}

#if 0
/**
* @brief  msg  schedule queue schedule process
* @param  void
* @note      This function should be call in while(1). it must be call periodly in a system time period
*                                               while(1){
*                                                       ......
*                                                       msg_schedule();
*                                               };
* */
uint8_t msg_schedule(queue_msg_t *msg_head)
{
    msg_t msg;
    memset(&msg, 0, sizeof(msg));

    uint16_t ret = msg_post_accept(msg_head, &msg);

    if(OS_ERR_NONE == ret)
    {
        if(msg.handler != NULL)
            msg.handler(msg.param);
    }

    return ret;
}
#endif


